# import libraries
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

%matplotlib inline 

# read data file
filename = '/Users/user/Desktop/Flying-Car-and-Autonomous-Flight-Engineer-ND/2-Motion_Planning/colliders.csv'
data = np.loadtxt(filename,delimiter=',',dtype='Float64',skiprows=2)
print(data)

[[-310.2389   -439.2315     85.5         5.          5.         85.5     ]
 [-300.2389   -439.2315     85.5         5.          5.         85.5     ]
 [-290.2389   -439.2315     85.5         5.          5.         85.5     ]
 ...
 [ 257.8061    425.1645      1.75852     1.292725    1.292725    1.944791]
 [ 293.9967    368.3391      3.557666    1.129456    1.129456    3.667319]
 [ 281.5162    354.4156      4.999351    1.053772    1.053772    4.950246]]

import os, sys
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath('/Users/user/Desktop/Flying-Car-and-Autonomous-Flight-Engineer-ND/2-Motion_Planning/planning_utils.py'))))
from planning_utils import create_grid

TARGET_ALTITUDE, SAFETY_DISTANCE = 5, 5

grid, north_offset, east_offset  = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE)
grid_zero, _, _ = create_grid(data, TARGET_ALTITUDE, 0)
print("North offset = {0}, east offset = {1}".format(north_offset, east_offset))

North offset = -316, east offset = -445

def visualize_grid(g, north_offset, east_offset):
    """
    Visualize the provided `g`
    """
    fig = plt.figure(figsize=(20,10))   
    plt.imshow(g, origin='lower') 
    plt.plot(-east_offset, -north_offset, 'r+')
    
    plt.xlabel('EAST')
    plt.ylabel('NORTH')
    plt.show()
    
visualize_grid(grid, north_offset, east_offset)

visualize_grid(grid+grid_zero, north_offset, east_offset)

def create_voxmap(data, voxel_size=5):
    """
    Returns a grid representation of a 3D configuration space
    based on given obstacle data.
    
    The `voxel_size` argument sets the resolution of the voxel map. 
    """
    # minimum and maximum north coordinates
    north_min = np.floor(np.amin(data[:, 0] - data[:, 3]))
    north_max = np.ceil(np.amax(data[:, 0] + data[:, 3]))

    # minimum and maximum east coordinates
    east_min = np.floor(np.amin(data[:, 1] - data[:, 4]))
    east_max = np.ceil(np.amax(data[:, 1] + data[:, 4]))

    # maximum altitude
    alt_max = np.ceil(np.amax(data[:, 2] + data[:, 5]))
    
    # given the minimum and maximum coordinates we can
    # calculate the size of the grid.
    north_size = int(np.ceil((north_max - north_min))) // voxel_size
    east_size = int(np.ceil((east_max - east_min))) // voxel_size
    alt_size = int(alt_max) // voxel_size

    # Create an empty grid
    voxmap = np.zeros((north_size, east_size, alt_size), dtype=np.bool)
    # Center offset for grid
    north_min_center = np.min(data[:, 0])
    east_min_center = np.min(data[:, 1])
    for i in range(data.shape[0]):
        # TODO: fill in the voxels that are part of an obstacle with `True`
        #
        # i.e. grid[0:5, 20:26, 2:7] = True
        north, east, alt, d_north, d_east, d_alt = data[i, :]
        obstacle = [
            int(north - d_north - north_min_center) // voxel_size,
            int(north + d_north - north_min_center) // voxel_size,
            int(east - d_east - east_min_center) // voxel_size,
            int(east + d_east - east_min_center) // voxel_size,
        ]
        height = int(alt + d_alt) // voxel_size
        voxmap[obstacle[0]:obstacle[1], obstacle[2]:obstacle[3], 0:height] = True

    return voxmap

voxmap = create_voxmap(data, 10)
print(voxmap.shape)

(92, 92, 21)

fig = plt.figure(figsize=(20,10))
ax = fig.gca(projection='3d')
ax.voxels(voxmap, edgecolor='k')
ax.set_xlim(voxmap.shape[0], 0)
ax.set_ylim(0, voxmap.shape[1])
ax.set_zlim(0, voxmap.shape[2])

plt.xlabel('North')
plt.ylabel('East')

plt.show()